import java.util.Vector;
import javax.bluetooth.RemoteDevice;
import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.SimpleNavigator;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;


public class MapMaker {
	public TouchSensor collisionDetector;
	public UltrasonicSensor eyes;
	public UltrasonicSensor leftSonar;
	public UltrasonicSensor rightSonar;
	public Motor leftMotor;
	public Motor rightMotor;
	public Motor neck;
	public DifferentialDriveKinematics ddk; 
	public BTserver btsvr;
	SimpleNavigator snav;
	static int ROTATION =200;


	// source: https://github.com/sarbogast/lejos-explorer/blob/master/src/org/epseelon/lejos/Explorer.java
	public MapMaker() 
	{
		leftMotor = Motor.B;
		rightMotor = Motor.C;
		//leftMotor.setSpeed(ROTATION);        
		//rightMotor.setSpeed(ROTATION);
		neck = Motor.A;
		collisionDetector = new TouchSensor(SensorPort.S1);
		eyes = new UltrasonicSensor(SensorPort.S2);
		leftSonar = new UltrasonicSensor(SensorPort.S3);
		rightSonar = new UltrasonicSensor(SensorPort.S4);
		//eyes.continuous();

		snav = new SimpleNavigator(3.5F, 15.5F, leftMotor, rightMotor);	


		//ddk = new DifferentialDriveKinematics(0.05, 0.05, 0.15);
		//ddk = new DifferentialDriveKinematics(0.035F,0.035F,0.15F);

		snav.setTurnSpeed(60);
		snav.setMoveSpeed(10);


		Button.ESCAPE.addButtonListener(new ButtonListener() {
			public void buttonPressed(Button button) { System.exit(0);   }
			public void buttonReleased(Button button) {}
		}
		);
	}


	//private double deg2rad(double deg) 	{ return ( deg * (Math.PI  / 180.0F) ) ; }

	public void UpdatePose()
	{		
		//SimpleNavigator snav = new SimpleNavigator(0.1, 0.15, leftMotor, rightMotor);


		//	ddk.nextStep(deg2rad( leftMotor.getRotationSpeed()),deg2rad( rightMotor.getRotationSpeed()));

		//	ddk.nextStep(Math.toRadians( leftMotor.getRotationSpeed()),  Math.toRadians(rightMotor.getRotationSpeed()));
		snav.updatePosition();


		LCD.drawString("Lspeed=" +  Math.toRadians( leftMotor.getRotationSpeed()),0,3);
		LCD.drawString("Rspeed=" +  Math.toRadians( rightMotor.getRotationSpeed()),0,4);
		LCD.drawString("X= " + snav.getX() , 0, 5);
		LCD.drawString("Y= " + snav.getY() , 0, 6);
		LCD.drawString("Orient= " + snav.getAngle() , 0, 7);
		btsvr.sendCommand(snav.getX(),snav.getY(),snav.getAngle() );		
		LCD.refresh();		
	}	


	public void loadMaster() 
	{
		LCD.drawString("Waiting...", 2, 1);
		BTConnection conn = Bluetooth.waitForConnection();
		conn.setIOMode(0); // Used when a pc connection is made	 
		LCD.clear();
		btsvr = new BTserver(conn); 	      // Start Bluetooth reader thread
		btsvr.start();
	}	

	public static void main(String[] args) {

		MapMaker explorer = new MapMaker();
		explorer.loadMaster();
		Behavior b1 = new MoveForward(explorer);
		Behavior b2 = new AvoidObstacles(explorer);
//		Behavior b3 = new DoSquare(explorer);
		Behavior b4 = new LeftSonarBehaviour(explorer);
		Behavior b5 = new RightSonarBehaviour(explorer);
		//Behavior b6 = new SideSonars(explorer);
		//Behavior[] behaviorList =  { b1, b4, b5, b2 };
		Behavior[] behaviorList =  { b1, b4, b5, b2 };

		Arbitrator arbitrator = new Arbitrator(behaviorList);

		//Button.waitForPress();
		arbitrator.start();		
	}

}